/*
 * Author: Ken Beck (kjb9089@rit.edu)
 *
 * Performs an A* search over a grid-based coordinate system. Returns a list
 * containing the shortest path from point A to point B.
 *
 */
#include "AStar.h"

// As these are static variables, we need to initialize them for future use.
map<const GridCoord*,double> AStar::g,AStar::h;

bool AStar::operator()(const GridCoord *s1,const GridCoord *s2) {
   return getF(s1) > getF(s2);
}

/** Private Methods **/

double AStar::getG(const GridCoord *at){
   if( g.find(at) == g.end() ){
      g[at] = 0;
   }
   return g[at];
}

double AStar::getH(const GridCoord *at){
   if( h.find(at) == h.end() ){
      h[at] = 0;
   }
   return h[at];
}

double AStar::getF(const GridCoord *at){
   return getG(at)+getH(at);
}

GridCoord::SearchState AStar::getState(const GridCoord *at){
   if( states.find(at) == states.end() ){
      states[at] = GridCoord::NEW;
   }
   return states[at];
}

void AStar::setG(const GridCoord *at,double val){
   g[at] = val;
}

void AStar::setH(const GridCoord *at,double val){
   h[at] = val;
}

void AStar::setH(const GridCoord *at,const GridCoord *goal){
   setH(at,calcHeuristic(at,goal));
}

void AStar::setState(const GridCoord *at,GridCoord::SearchState val){
   states[at] = val;
}

inline double AStar::calcHeuristic(const GridCoord *from,const GridCoord *to){
   return grid->dist_between(from,to);
}

list<GridCoord*> AStar::get_path(GridCoord *at){
   list<GridCoord*> path;
   while(at != 0){
      path.push_front(at);
      at = at->from;
   }
   return path;
}


/** Public Methods **/

AStar::AStar(Grid *_grid) : grid(_grid){}

list<GridCoord*> AStar::search(Coord start,Coord goal){
   return this->search((*grid)[start],(*grid)[goal]);
}

list<GridCoord*> AStar::search(DCoord start,DCoord goal){
   return this->search((*grid)[start],(*grid)[goal]);
}

void AStar::resort_queue(astar_priority_queue *queue){
   list<GridCoord*> tmp;
   GridCoord* a;
   while(!queue->empty()){
      a = queue->top();
      tmp.push_back(a);
      queue->pop();
   }  
   
   while(!tmp.empty()){
      a = tmp.front();
      queue->push(a);
      tmp.pop_front();
   }
}


list<GridCoord*> AStar::search(GridCoord *start,GridCoord *goal){
   if( start == 0 || goal == 0 ){
      //cout << "Start or goal empty" << endl;
      return list<GridCoord*>();
   }
   if( start->cVal > 0.5 || goal->cVal > 0.5 ){
      //cout << "Starting/wanting to go into a wall..." << endl;
      return list<GridCoord*>();
   }
   
   g = map<const GridCoord*,double>();
   h = map<const GridCoord*,double>();
   states = map<const GridCoord*,GridCoord::SearchState>();
   
   //start = grid->get_gridCoordFromCSpace(start->cPos);
   //goal = grid->get_gridCoordFromCSpace(goal->cPos);
   
   
   GridCoord *x;
   astar_priority_queue openedSet;
   list<GridCoord*> neighbors;
   list<GridCoord*>::iterator it;

   setH(start,goal);
   setG(start,0);
   
   openedSet.push(start);

   while(openedSet.size() > 0){
      x = openedSet.top();
      
      if( x->cPos == goal->cPos ){
         return get_path(x);
      }
      
      openedSet.pop();
      
      setState(x,GridCoord::CLOSED);
      
      bool resortOpened = false;
      neighbors = grid->get_neighbors(x);
      for(it = neighbors.begin();it!=neighbors.end();it++){
         GridCoord *y = *it;
         
         // Ignore state if it's already been looked at or if 
         // the chance that it's an obstacle is greater than 50%
         if( getState(y) == GridCoord::CLOSED || y->cVal > 0.5 ){
            continue;
         }else{
            double gScore = getG(x)+grid->dist_between(x,y);
            
            if( x->from != y && (gScore < getG(y) || getG(y) == 0)){
               setG(y,gScore);
               setH(y,goal);

               y->from = x;
               
               if( getState(y) != GridCoord::OPEN ){
                  openedSet.push(y);
                  setState(y,GridCoord::OPEN);
               }else{ // Have to re-sort y based on changed heuristic
                  resortOpened = true;
               }
            }
         }
      }

      if( resortOpened ){
         resort_queue(&openedSet);
      }
   }
   
   return list<GridCoord*>();
}
